#include "ros/ros.h"
#include "roboard_drivers/adc_val.h"
#include "roboard_drivers/sonar.h"
#include "std_msgs/Int32.h"
#include "std_msgs/String.h"
#include "gr8/Distance.h"
#include <stdlib.h>
#include <math.h>

using namespace roboard_drivers;
using namespace gr8;
ros::Publisher distancePub;

float sensor0 = 0;
float sensor1 = 0;
float sensor2 = 0;
float sensor3 = 0;
float sensor4 = 0;
float sensor5 = 0;
float sonarVal = 0;

bool sonarHasAValue = false;

/* This node is doing :
	-> Receive values from IR sensors and from sonar
	-> Use a look up table and interpolation to find the distance from the value given by the IR sensors
	-> Use 10 measurements for IR sensors, remove the highest and the lowest values and then do the averag
*/


// Look up table for short range sensors
// Values for corresponding distance (in cm) : 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 20 25 30
float distanceCentimetersShort[] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 20, 25, 30};
int sensorCalibrationShort[4][19] = { 357, 318, 285, 260, 238, 219, 204, 191, 180, 169, 155, 149, 142, 138, 132, 129, 116, 111, 108,
			     357, 321, 282, 257, 235, 221, 201, 191, 183, 172, 162, 152, 144, 139, 133, 128, 112, 106, 100,
			     522, 448, 391, 346, 309, 282, 253, 234, 215, 200, 188, 177, 164, 158, 150, 143, 119, 105, 98,
		             587, 504, 444, 388, 342, 303, 276, 253, 233, 214, 202, 190, 182, 171, 162, 155, 124, 102, 88
			    };

// Look up table for long range sensors
//float distanceCentimetersLong[] = { 4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 24, 26, 28, 30, 35, 40, 45, 50, 55, 60};
float distanceCentimetersLong[] = { 5, 10, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60};
int sensorCalibrationLong[2][12] = { 413, 313, 250, 207, 179, 158, 143, 129, 119, 111, 107, 102,
				     411, 311, 248, 206, 178, 157, 140, 128, 116, 107, 100, 92
			    };

// Interpolation function for short range sensors
float linearizeFunctionShort(int volt, int sensorNumber)
{
	int closerValue = sensorCalibrationShort[sensorNumber][0];
	int i = 0;
	while (closerValue > volt) {
		i++;
		if (i<19) closerValue = sensorCalibrationShort[sensorNumber][i];
		else closerValue = 0;
	}
	if (volt == closerValue) return distanceCentimetersShort[i];
	else {
		switch (i) 
		{	case 0:
			return 0;
			break;

			case 19:
			return distanceCentimetersShort[18];
			break;

			default :
			float approx = ((float) volt - (float) closerValue)*((distanceCentimetersShort[i-1]-distanceCentimetersShort[i])/((float)sensorCalibrationShort[sensorNumber][i-1]- (float) sensorCalibrationShort[sensorNumber][i])) + distanceCentimetersShort[i];
			return approx;
			break;
		}
		
	}

	
}

// Interpolation function for long range sensors
float linearizeFunctionLong(int volt, int sensorNumber)
{
	int closerValue = sensorCalibrationLong[sensorNumber][0];
	int i = 0;
	while (closerValue > volt) {
		i++;
		if (i<12) closerValue = sensorCalibrationLong[sensorNumber][i];
		else closerValue = 0;
	}

	if (volt == closerValue) return distanceCentimetersLong[i];
	else {
		switch (i) 
		{	case 0:
			return 0;
			break;

			case 12:
			return distanceCentimetersLong[11];
			break;

			default :
			float approx = ((float) volt - (float) closerValue)*((distanceCentimetersLong[i-1]-distanceCentimetersLong[i])/((float)sensorCalibrationLong[sensorNumber][i-1]- (float) sensorCalibrationLong[sensorNumber][i])) + distanceCentimetersLong[i];
			return approx;
			break;
		}
		
	}

	
}

// Function to receive the value from the sonar
void SonarCallback(const sonar::ConstPtr& msg) {

	sonarVal = msg->distance;
	sonarHasAValue = true;
}

// Function to receive the values from the IR sensors
void IRSensorsCallback(const adc_val::ConstPtr& msg)
{
	// Short range IR		
	sensor0 = linearizeFunctionShort(msg->val0, 0);
	sensor1 = linearizeFunctionShort(msg->val1, 1);
	sensor2 = linearizeFunctionShort(msg->val2, 2);
	sensor3 = linearizeFunctionShort(msg->val3, 3);
	
	// Long range IR
	sensor4 = linearizeFunctionLong(msg->val4, 0);
	sensor5 = linearizeFunctionLong(msg->val5, 1);

}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "Sensors");
	ros::NodeHandle n;
	ros::Subscriber sub = n.subscribe("roboard/adc", 1, IRSensorsCallback);
	ros::Subscriber subSonar = n.subscribe("roboard/sonar", 1, SonarCallback);
	distancePub = n.advertise<Distance>("sensors/distance", 1);
	ros::Publisher chatter_pub = n.advertise<std_msgs::Int32>("roboard/adc_interval", 1);

	std_msgs::Int32 msg;		
	msg.data = 100;
	ros::Rate loop_rate(100);

	double sensor_cache[10][6];
	int count = 0;

	while (ros::ok())
	{
		chatter_pub.publish(msg);		
		ros::spinOnce();
		
		/* Take 10 measures, remove the extreme values, and do the average */

		// Short range IR
		sensor_cache[count][0] = sensor0;
		sensor_cache[count][1] = sensor1;
		sensor_cache[count][2] = sensor2;
		sensor_cache[count][3] = sensor3;

		// Long range IR
		sensor_cache[count][4] = sensor4;
		sensor_cache[count][5] = sensor5;


		if(count == 10) {
			Distance dist;

			double sensor_filtered[6] = {0,0,0,0,0,0};
			
			for(int i = 0; i < 6; i++)
			{
				double max = 0;
				double min = 1000;
				for(int k = 0; k < 10; k++)
				{
					if(sensor_cache[k][i] > max)
						max = sensor_cache[k][i];
					if(sensor_cache[k][i] < min)
						min = sensor_cache[k][i];
					sensor_filtered[i] += sensor_cache[k][i];
				}
				sensor_filtered[i] = (sensor_filtered[i] - max - min) / 8;
			}
		
			/* Publish the distances */

			// IR
			dist.val0 = sensor_filtered[0];
		  	dist.val1 = sensor_filtered[1];
		  	dist.val2 = sensor_filtered[2];
		  	dist.val3 = sensor_filtered[3];
		  	dist.val4 = sensor_filtered[4];
		  	dist.val5 = sensor_filtered[5];

			//Sonar
			if (sonarHasAValue) {
				dist.sonar = sonarVal;
				sonarHasAValue = false;
			} else {
				dist.sonar = -1.0;
			}

			distancePub.publish(dist);
			count = 0;
		}

		count++;

		loop_rate.sleep();
	}
	return 0;
}
